// -*- mode: c++; indent-tabs-mode: nil; -*-
//
//The Biomolecule Toolkit (BTK) is a C++ library for use in the
//modeling, analysis, and design of biological macromolecules.
//Copyright (C) 2001-2006, Chris Saunders <ctsa@users.sourceforge.net>,
//                         Tim Robertson <kid50@users.sourceforge.net>
//
//This program is free software; you can redistribute it and/or modify
//it under the terms of the GNU Lesser General Public License as published
//by the Free Software Foundation; either version 2.1 of the License, or (at
//your option) any later version.
//
//This program is distributed in the hope that it will be useful,  but
//WITHOUT ANY WARRANTY; without even the implied warranty of
//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
//Lesser General Public License for more details.
//
//You should have received a copy of the GNU Lesser General Public License
//along with this program; if not, write to the Free Software Foundation,
//Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

#include <btk/core/algorithms/rmsd.hpp>

void 
BTK::ALGORITHMS::internal::
fast_solve_3D_eigenvectors(BTK::MATH::BTKMatrix & M,
			   double const evalues[3],
			   BTK::MATH::BTKVector evecs[3])
{
  unsigned ev1,ev2;
  using BTK::MATH::equivalent;
  using BTK::MATH::cross;
  using boost::numeric::ublas::row;


  if (!equivalent(evalues[0],evalues[1])) {
    // eigenvalues 1 and 2 are unique, so use these.
    ev1 = 0;
    ev2 = 1;
  } else if (!equivalent(evalues[1],evalues[2])) {
    // eigenvalues 1 and 2 are identical, but 2 and 3 are unique.
    ev1 = 1;
    ev2 = 2;
  } else {
    // all three eigenvalues equal. eigenvectors are trivial.
    evecs[0][0] = 1;
    evecs[0][1] = evecs[0][2] = 0;

    evecs[1][1] = 1;
    evecs[1][0] = evecs[1][2] = 0;

    evecs[2][2] = 1;
    evecs[2][0] = evecs[2][1] = 0;

    return;
  }

  // compute first two eigenvectors
  // (the vectors corresponding to the largest unequal eigenvalues)
  for (unsigned i = ev1; i <= ev2; ++i) {
    M(0,0) -= evalues[i];
    M(1,1) -= evalues[i];

    evecs[i] = cross(row(M,0),row(M,1));
    evecs[i] /= norm_2(evecs[i]);

    M(0,0) += evalues[i];
    M(1,1) += evalues[i];
  }

  // compute third eigenvector as cross of first two,
  // in order to guarantee an orthogonal system.
  if (ev2 == 1) {
    // third vector = vector 3
    evecs[2] = cross(evecs[0],evecs[1]);
  } else if (ev2 == 2) {
    // third vector = vector 1
    evecs[0] = cross(evecs[1],evecs[2]);
  }
}

void 
BTK::ALGORITHMS::internal::
fast_solve_3D_eigenvalues(BTK::MATH::BTKMatrix const & M,
			  double e_vals[3])
{
  //  get elements we need from symmetric matrix M:
  //   (funky matrix naming relic of first attempt using pivots)
  //          matrix = d0 e0 f0
  //                      d1 e1
  //                         d2
  //
  // (also: Normalize matrix so that cubic root algorithm can handle it.)
  double d0 = M(0,0);
  double e0 = M(0,1) / d0;
  double f0 = M(0,2) / d0;
  double d1 = M(1,1) / d0;
  double e1 = M(1,2) / d0;
  double d2 = M(2,2) / d0;

  // analytic solution of cubic roots using method of Viete
  // (see Numerical Recipes)
  {
    // solving for eigenvalues as det(M-I*lambda) = 0
    // yields the values below corresponding to:
    // lambda**3 + B*lambda**2 + C*lambda + D = 0
    //   (given that d0=1.)
    double B = -1-d1-d2;
    double C = d1+d2+d1*d2-e0*e0-f0*f0-e1*e1;
    double D = e0*e0*d2+e1*e1+f0*f0*d1-d1*d2-2*e0*f0*e1;

    double q = (B*B-3.0*C)/9.0;
    double q3 = q*q*q;
    double r = (2.0*B*B*B-9.0*B*C+27.0*D)/54.0;
    double theta = acos(r/sqrt(q3));

    e_vals[0] = e_vals[1] = e_vals[2] = -2.0*sqrt(q);

    e_vals[0] *= cos(theta/3.0);
    e_vals[1] *= cos((theta+2*BTK::MATH::PI)/3.0);
    e_vals[2] *= cos((theta-2*BTK::MATH::PI)/3.0);

    double b = B/3.0;

    e_vals[0] -= b;
    e_vals[1] -= b;
    e_vals[2] -= b;
  }

  // undo the d0 norm to get eigenvalues
  e_vals[0] = e_vals[0] * d0;
  e_vals[1] = e_vals[1] * d0;
  e_vals[2] = e_vals[2] * d0;
}
